#ifndef ZEROERR_HPP
#define ZEROERR_HPP

#include "dispatch.h"
#include "can.hpp"
#include "kinco.hpp"
#include <iostream>
#include <string>
#include <math.h>

namespace CANalyst2
{
    // #define DEC_TO_AP_DIV   20.48
    // #define DEC_TO_RMS_DIV  28.96
    // #define ACC_DEC_MULT    0.016384
    // #define RPM_TO_DEC_MULT 0.273067
    // struct __attribute__((__packed__)) MotorCtrlCmd
    // {
    //     uint16_t ctrl;
    //     int32_t target;
    // };

    // struct __attribute__((__packed__)) MotorStatusFbk
    // {
    //     uint16_t status;
    //     int32_t pos;
    //     int16_t current;
    // };

    // enum Servo_Mode {
    //     IMMDE_VEL_MODE = -3,
    //     POS_MODE = 1,
    //     VEL_MODE = 3,
    //     TORQUE_MODE = 4,
    //     HOME_MODE = 6,
    // };

    // enum Servo_Status {
    //     INVALID,
    //     SWITCH_ON_DISABLED,
    //     READY_TO_SWITCH_ON,
    //     SWITCHED_ON,
    //     OPERATION_ENABLED,
    //     FAULT,
    // };

    class ZeroErr 
    {
    private:
        uint8_t slave_id = 1;
        int can_ch = 1;
        bool is_pos_mode;
        int resolution;
        uint32_t profiled_vel;
        uint32_t profiled_acc;
        uint32_t profiled_dec;
        bool _is_fault;
        bool is_motor_on = false;
        uint16_t _error_code;
        uint16_t _error_cnt;
        int _error_req;

        MotorStatusFbk status_fbk;
        Servo_Status current_status = Servo_Status::INVALID;

        std::shared_ptr<CANDispatch> can;
        int processRecvTPDOFrame(const CanStamped &recv_frame);
        int processRecvEmcyFrame(const CanStamped &recv_frame);
        void parseStatusWord(uint16_t status);

    public:
        ZeroErr(std::shared_ptr<CANDispatch> _can, int ch, int _slave_id, bool _is_pos_mode = true, int _resolution = 524288, 
                uint32_t _profiled_vel = 10, uint32_t _profiled_acc = 10, uint32_t _profiled_dec = 5);
        ~ZeroErr();
        
        void SendStopRemoteNode();
        void SendResetComm();
        void SendStartRemoteNode();
        int SendEnableMotorPDO();
        int SendDisableMotorPDO();
        void Setup();
        int32_t SendSetTargetPosSDO(float degree);

        bool SendSetWorkModeSDO(Servo_Mode mode);
        bool SendSetTargetVelocitySDO(int32_t rpm);
        bool SendSetProfiledVelocitySDO(uint32_t deg_per_s);
        bool SendSetProfiledVelocityPDO(uint32_t rpm);
        bool SendSetProfiledAccSDO(uint32_t deg_per_ss);
        bool SendSetProfiledDecSDO(uint32_t deg_per_ss);
        void SendSetCWordShutDown();
        void SendSetCWordSwitchOn();
        void SendSetCWordEnOperation();
        void SendSetCWordClrFault();
        void SendSetCWordClrZero();
        void SendSetCWordCmdNonImmTriggered(float degree);
        void SendSetCWordCmdImmTriggered(float degree);

        void SendAcquireValidWorkModeSDO(int8_t &mode, bool &is_acq);
        void SendAcquireServoWorkModeSDO(int8_t &mode, bool &is_acq);
        void SendAcquireStatusWordSDO(uint16_t &status, bool &is_acq);
        void SendAcquireCtrlWordSDO(uint16_t &ctrl, bool &is_acq);
        void SendAcquireTargetPosSDO(int32_t &pos, bool &is_acq);
        void SendAcquireActualPosSDO(int32_t &pos, bool &is_acq);
        void SendAcquireTargetVelocitySDO(int32_t &vel, bool &is_acq);
        void SendAcquireActualVelocitySDO(int32_t &vel, bool &is_acq);
        void SendAcquireProfiledVelSDO(uint32_t &acc, bool &is_acq);
        void SendAcquireProfiledAccSDO(uint32_t &acc, bool &is_acq);
        void SendAcquireProfiledDecSDO(uint32_t &dec, bool &is_acq);
        void SendAcquireActualCurrentSDO(int16_t &current, bool &is_acq);
        void SendQuickStopSDO();
        bool SendResumeFromQuickStopSDO();
        void SendReadQuickStopOptionSDO(uint16_t &option, bool &is_acq);
        void SendWriteQuickStopOptionCode(int16_t option);

        void SendMoveTargetPosPDO(float degree);
        void SendMoveTargetPosPDO(float pos, uint32_t rpm);

        void SendSyncFrame();
        void SendSetPosModeSDO();
        int32_t GetCurPosPDO();
        bool WaitState(Servo_Status target_state, int tries = 100, int delay = 10000);
        MotorStatusFbk GetCurFBKPDO(float &pos, float &ap, float &rms);
        Servo_Status GetCurStatusPDO();
        int16_t GetCurrentPDO(float &ap, float &rms);
        inline bool GetMode() { return is_pos_mode; }
        inline void SetMode(bool is_pos) { is_pos_mode = is_pos; }
        inline uint16_t GetErrorCode() { return _error_code; }
        inline float PosToDegree(int32_t pos) { return (pos * 360.0f) / resolution; }
    };
}

#endif